//testfile
//All includes
#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include "saint_nr_9/MotCom.h"
#include "saint_nr_9/IRdata.h"
#include <unistd.h>
#include <math.h>
#include <stdio.h>

ros::Subscriber ir_sub;
ros::Publisher moto_pub;

saint_nr_9::IRdata ir_data;

void IRCallback(const saint_nr_9::IRdata::ConstPtr& data){
	ir_data = *data;
}

int main(int argc, char **argv){
	ros::init(argc, argv, "Test"); //Creates a node named test
	ros::NodeHandle n;
	ir_sub = n.subscribe("ir_info", 1000, IRCallback);
	moto_pub = n.advertise<saint_nr_9::MotCom>("/serial/motor_control", 1);
	saint_nr_9::MotCom moto_command;
	moto_command.command = moto_command.MOVE_FORWARD;
	ros::Rate loop_rate(100);
	sleep(2);
	//moto_pub.publish(moto_command);
	//sleep(1);
	ROS_INFO("Let's go... Move forward");
	//while(ros::ok()){

		ROS_INFO("IS THIS A U TURN?");
		moto_command.command = moto_command.MOVE_FORWARD_DIST;
		moto_command.dist_or_angle = 0.3;
		moto_pub.publish(moto_command);
		ros::spinOnce();
	 	loop_rate.sleep();
	 	sleep(6);
	 	
		moto_command.command = moto_command.TURN_RIGHT;
		moto_command.dist_or_angle = 90;
		moto_pub.publish(moto_command);
		ros::spinOnce();
	 	loop_rate.sleep();
	 	sleep(6);
	 	
	 	moto_command.command = moto_command.MOVE_FORWARD_DIST;
		moto_command.dist_or_angle = 0.3;
		
		moto_pub.publish(moto_command);
		ros::spinOnce();
                loop_rate.sleep();
                ROS_INFO("DONE");
	//} 
}
